# -*- coding = utf-8 -*-
# @Time : 2023/9/22 9:12
# @Author : 龙王赘婿_彪少
# @File : PTC200_machine.py
# @Software: PyCharm

# 功能：模拟底盘向上发数据，看到底是谁的代码问题


from socket import *
from threading import Thread
import struct
import time
import binascii
from datetime import datetime, timedelta


class MYTCP:
    current_time = datetime.now()
    last_time = datetime.now()

    TX_HEART = True
    TX_ULT = True
    TX_OTHER = True
    TX_ENCODER = True
    TX_SPEED = True
    TX_IMU = True

    TX_HEART_FREQUENCY = 1  # Hz
    TX_ULT_FREQUENCY = 20  # Hz
    TX_OTHER_FREQUENCY = 5  # Hz
    TX_ENCODER_FREQUENCY = 40  # Hz
    TX_SPEED_FREQUENCY = 30  # Hz
    TX_IMU_FREQUENCY = 50  # Hz

    def __init__(self):
        # self.ip = "192.168.8.31"
        # self.ip = "192.168.111.250"
        self.ip = "127.0.0.1"
        self.port = 9000

        self.server_socket = socket(AF_INET, SOCK_STREAM)  # 建立TCP套接字
        self.server_socket.bind((self.ip, self.port))  # 本机监听端口
        self.server_socket.listen(5)
        print("等待接收连接！")
        self.client_socket, self.client_info = self.server_socket.accept()
        print("一个客户端建立连接成功！")
        # self.client_socket = socket(AF_INET, SOCK_STREAM)
        # self.client_socket.connect((self.ip, self.port))
        t1 = Thread(target=self.recv_thread)
        t2 = Thread(target=self.send_thread)
        t1.start()
        t2.start()
        t1.join()
        t2.join()
        self.client_socket.close()

    # 接收线程
    def recv_thread(self):
        while True:
            recv_data = self.client_socket.recv(1024)  # 最大接收1024字节
            self.print_recv_data(recv_data)
            # self.current_time = datetime.now()
            # if self.current_time - self.last_time > timedelta(seconds=1):
            #     print(self.current_time - self.last_time)
            #     print("*"*100)
            # self.last_time = self.current_time
            # self.decode_recv_data(recv_data)

    @staticmethod
    def print_recv_data(raw_data):
        print(f"recv：len = {len(raw_data)} data =", end=" ")
        for i in range(len(raw_data)):
            print(f"{raw_data[i]:02x}", end=" ")
        print()

    # 解析编码器和速度报文
    @staticmethod
    def decode_recv_data(raw_data):
        # 测试接收：01 06 60 40 00 01 57 de
        s = struct.Struct("<2B2H1I8f4i1H2B")
        up = s.unpack(raw_data)
        head = up[0]
        version = up[1]
        type = up[2]
        len = up[3]
        count = up[4]

        speed_lf = up[5]
        speed_rf = up[6]
        speed_lb = up[7]
        speed_rb = up[8]

        degrees_lf = up[9]
        degrees_rf = up[10]
        degrees_lb = up[11]
        degrees_rb = up[12]

        line_lf = up[13]
        line_rf = up[14]
        line_lb = up[15]
        line_rb = up[16]

        crc = up[17]
        end1 = up[18]
        end2 = up[19]

        # print(f"head = {head:02x} version = {version:02x} type = {type:04x}")
        # print(f"speed: lf = {speed_lf:f} rf = {speed_rf:f} lb = {speed_lb:f} rb = {speed_rb:f}")
        # print(f"degree: lf = {degrees_lf:f} rf = {degrees_rf:f} lb = {degrees_lb:f} rb = {degrees_rb:f}")
        # print(f"line: lf = {line_lf:d} rf = {line_rf:d} lb = {line_lb:d} rb = {line_rb:d}")

    def ackermann(self):
        mode = 1
        v = -0.5
        w = 0.5
        r = 0
        deg = 0
        buf = [0xff, 0xfe, 0x11, 31, 0x00, mode, v, w, r, deg]
        s = struct.Struct("<2B2H1I1B4f")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    def dipan_pc_heart(self, time_count):
        protocol_type = 0x00
        data_length = 20

        state = 0
        error1 = 0
        version1 = 1
        version2 = 2
        version3 = 0
        dynamic = 0

        buf = [0xff, 0xfe, protocol_type, data_length, time_count, state, error1, version1, version2, version3, dynamic]
        s = struct.Struct("<2B2HI6B")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    def dipan_pc_ult(self, time_count):
        protocol_type = 0x01
        data_length = 30

        ult_lf = 10
        ult_rf = 10
        ult_lb = 10
        ult_rb = 10

        buf = [0xff, 0xfe, protocol_type, data_length, time_count, ult_lf, ult_rf, ult_lb, ult_rb]
        s = struct.Struct("<2B2HI4f")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    def dipan_pc_other(self, time_count):
        protocol_type = 0x02
        data_length = 28

        safe_edge_front = 1
        safe_edge_back = 1
        state_line_lf = 2
        state_line_rf = 2
        state_line_lb = 2
        state_line_rb = 2
        state_roll_lf = 1
        state_roll_rf = 2
        state_roll_lb = 1
        state_roll_rb = 2
        vel_controller_state = 1
        rev1 = 0
        rev2 = 0
        rev3 = 0

        buf = [0xff, 0xfe, protocol_type, data_length, time_count,
               safe_edge_front,
               safe_edge_back,
               state_line_lf,
               state_line_rf,
               state_line_lb,
               state_line_rb,
               state_roll_lf,
               state_roll_rf,
               state_roll_lb,
               state_roll_rb,
               vel_controller_state,
               rev1,
               rev2,
               rev3,
               ]
        s = struct.Struct("<2B2HI14B")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    def dipan_pc_encoder(self, time_count):
        protocol_type = 0x03
        data_length = 62

        speed_lf = 0.1
        speed_rf = 0.1
        speed_lb = 0.1
        speed_rb = 0.1
        degrees_lf = 20.0
        degrees_rf = 20.0
        degrees_lb = 20.0
        degrees_rb = 20.0
        line_lf = 10000
        line_rf = 10000
        line_lb = 10000
        line_rb = 10000

        buf = [0xff, 0xfe, protocol_type, data_length, time_count,
               speed_lf,
               speed_rf,
               speed_lb,
               speed_rb,
               degrees_lf,
               degrees_rf,
               degrees_lb,
               degrees_rb,
               line_lf,
               line_rf,
               line_lb,
               line_rb,
               ]
        s = struct.Struct("<2B2HI8f4I")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    def dipan_pc_speed(self, time_count):
        protocol_type = 0x04
        data_length = 40

        position_x = 0
        position_y = 0
        linear_x = 0
        linear_y = 0
        angular_z = 0
        yaw = 0
        mode = 1
        stop_flag = 0

        buf = [0xff, 0xfe, protocol_type, data_length, time_count,
               position_x,
               position_y,
               linear_x,
               linear_y,
               angular_z,
               yaw,
               mode,
               stop_flag,
               ]
        s = struct.Struct("<2B2HI6f2B")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    def dipan_pc_reply(self, time_count, typ, tim):
        protocol_type = 0x05
        data_length = 21

        res = 0

        buf = [0xff, 0xfe, protocol_type, data_length, time_count, res, typ, tim]
        s = struct.Struct("<2B2HIBHI")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    def dipan_pc_imu(self, time_count):
        protocol_type = 0x06
        data_length = 66

        timestamp = 1000
        roll = 1
        pitch = 2
        yaw = 3
        acc_x = 4
        acc_y = 5
        acc_z = 6
        angular_x = 7
        angular_y = 8
        angular_z = 9
        magnetic_x = 10
        magnetic_y = 11
        magnetic_z = 12

        buf = [0xff, 0xfe, protocol_type, data_length, time_count,
               timestamp,
               roll,
               pitch,
               yaw,
               acc_x,
               acc_y,
               acc_z,
               angular_x,
               angular_y,
               angular_z,
               magnetic_x,
               magnetic_y,
               magnetic_z,
               ]
        s = struct.Struct("<2B2H2I12f")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    def dipan_pc_status(self, time_count):
        protocol_type = 0x07
        data_length = 22

        switch_flag = 0
        switch_effective = 0

        buf = [0xff, 0xfe, protocol_type, data_length, time_count, switch_flag, switch_effective]
        s = struct.Struct("<2B2H3I")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    @staticmethod
    def crc16_cal(datalist):
        # from "https://code84.com/461550.html"
        test_crc = 0xFFFF
        poly = 0xa001
        # poly=0x8005
        numl = len(datalist)
        for num in range(numl):
            data = datalist[num]
            test_crc = (data & 0xFF) ^ test_crc
            for bit in range(8):
                if (test_crc & 0x1) != 0:
                    test_crc >>= 1
                    test_crc ^= poly
                else:
                    test_crc >>= 1
        return test_crc

    # 发送线程
    def send_thread(self):
        current_tick = 0
        time_count = 0

        heart_tick = datetime.now()
        other_tick = datetime.now()
        encoder_tick = datetime.now()
        ult_tick = datetime.now()
        speed_tick = datetime.now()
        imu_tick = datetime.now()

        while True:
            # 给服务端发消息
            # send_data = self.ackermann()
            # self.client_socket.send(send_data)
            # n2 = binascii.b2a_hex(send_data)
            # print(f"n2={n2},type={type(n2)}")

            self.current_time = datetime.now()

            if self.current_time - heart_tick > timedelta(milliseconds=1000 / self.TX_HEART_FREQUENCY) and self.TX_HEART:
                time_count = time_count + 1
                self.client_socket.send(self.dipan_pc_heart(time_count))
                heart_tick = self.current_time

            if self.current_time - ult_tick > timedelta(milliseconds=1000 / self.TX_ULT_FREQUENCY) and self.TX_ULT:
                time_count = time_count + 1
                self.client_socket.send(self.dipan_pc_ult(time_count))
                ult_tick = self.current_time

            if self.current_time - other_tick > timedelta(milliseconds=1000 / self.TX_OTHER_FREQUENCY) and self.TX_OTHER:
                time_count = time_count + 1
                self.client_socket.send(self.dipan_pc_other(time_count))
                other_tick = self.current_time

            if self.current_time - encoder_tick > timedelta(milliseconds=1000 / self.TX_ENCODER_FREQUENCY) and self.TX_ENCODER:
                time_count = time_count + 1
                self.client_socket.send(self.dipan_pc_encoder(time_count))
                encoder_tick = self.current_time

            if self.current_time - speed_tick > timedelta(milliseconds=1000 / self.TX_SPEED_FREQUENCY) and self.TX_SPEED:
                time_count = time_count + 1
                self.client_socket.send(self.dipan_pc_speed(time_count))
                speed_tick = self.current_time

            if self.current_time - imu_tick > timedelta(milliseconds=1000 / self.TX_IMU_FREQUENCY) and self.TX_IMU:
                time_count = time_count + 1
                self.client_socket.send(self.dipan_pc_imu(time_count))
                imu_tick = self.current_time


if __name__ == "__main__":
    obj = MYTCP()
